#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped


def pose_msg_publish(pose_data, timestamp, publisher):
    # 创建 PoseStamped 消息
    pose_msg = PoseStamped()

    # 设置 header
    pose_msg.header.seq = pose_data['header']['seq']
    pose_msg.header.stamp.secs = pose_data['header']['stamp']['secs']
    pose_msg.header.stamp.nsecs = pose_data['header']['stamp']['nsecs']
    pose_msg.header.frame_id = pose_data['header']['frame_id']

    # 设置 pose
    pose_msg.pose.position.x = pose_data['pose']['position']['x']
    pose_msg.pose.position.y = pose_data['pose']['position']['y']
    pose_msg.pose.position.z = pose_data['pose']['position']['z']

    pose_msg.pose.orientation.x = pose_data['pose']['orientation']['x']
    pose_msg.pose.orientation.y = pose_data['pose']['orientation']['y']
    pose_msg.pose.orientation.z = pose_data['pose']['orientation']['z']
    pose_msg.pose.orientation.w = pose_data['pose']['orientation']['w']

    # 发布消息
    publisher.publish(pose_msg)


if __name__ == "__main__":
    # 初始化 ROS 节点
    rospy.init_node('pose_publisher_node')

    # 创建一个 Publisher，发布到 /paper_pose 话题
    paper_pose_pub = rospy.Publisher('/paper_pose', PoseStamped, queue_size=10)

    # 创建一个 Publisher，发布到 /yogurt_pose 话题
    yogurt_pose_pub = rospy.Publisher('/yoger_pose', PoseStamped, queue_size=10)

    # 模拟数据：/paper_pose 的位姿信息
    paper_pose_data = {
        'header': {
            'seq': 15850,
            'stamp': {
                'secs': 1736498668,
                'nsecs': 96731185
            },
            'frame_id': "base_link"
        },
        'pose': {
            'position': {
                'x': 0.52,
                'y': -0.18,
                'z': 0.72
            },
            'orientation': {
                'x': -1,
                'y': -1,
                'z': -1,
                'w': -1
            }
        }
    }

    # 模拟数据：/yogurt_pose 的位姿信息
    yogurt_pose_data = {
        'header': {
            'seq': 15851,  # 注意：seq 需要唯一
            'stamp': {
                'secs': 1736498669,  # 时间戳可以不同
                'nsecs': 96731186
            },
            'frame_id': "base_link"
        },
        'pose': {
            'position': {
                'x': 0.59,  # 不同的位置
                'y': 0.038,
                'z': 0.723
            },
            'orientation': {
                'x': -1,  # 不同的姿态
                'y': -1,
                'z': -1,
                'w': -1
            }
        }
    }

    # 发布消息
    rate = rospy.Rate(10)  # 10Hz
    while not rospy.is_shutdown():
        # 发布 /paper_pose 消息
        pose_msg_publish(paper_pose_data, paper_pose_data['header']['stamp'], paper_pose_pub)

        # 发布 /yogurt_pose 消息
        pose_msg_publish(yogurt_pose_data, yogurt_pose_data['header']['stamp'], yogurt_pose_pub)

        rate.sleep()